#include <algorithm>
#include <cstdlib>
#include <vector>
#include <iostream>
#include <fstream>
#include <utility>

#include "sdopencv/PHaarType.hpp"

using namespace std;

#include <highgui.h>

namespace sdopencv
{
    EdgeType1::EdgeType1 ()
    {
        nparams = 2;
        can_swap = true;
        can_rotate90 = false; // either swapping or rotating by 90 degrees only
        can_rotate180 = false;
    }


    // compute the size
    CvSize EdgeType1::size(const vector<int> & params)
    {
        CvSize sz;
        if(params[0] > 0 && params[1] >= 0)
        {
            sz.height = params[1] + (params[0] << 1);
            sz.width = params[0] << 1;
        }
        else
            sz.width = sz.height = -1;
        return sz;
    }


    // generate a PHaar feature with its top left at (x,y) and with its parameters given
    PHaarFeature EdgeType1::generate(int x, int y, const vector<int> & params)
    {
        PHaarFeature hf;
        CvSize sz = size(params);

        hf.p[0].push_back(cvPoint(x + params[0], y));
        hf.p[0].push_back(cvPoint(x + params[0], y + sz.height));
        hf.p[0].push_back(cvPoint(x, y + params[0] + params[1]));
        hf.p[0].push_back(cvPoint(x, y + params[0]));
        hf.p[0].push_back(cvPoint(x + params[0], y));
        hf.A[0] = params[0]*(params[0]+params[1]);

        hf.p[1].push_back(cvPoint(x + params[0], y));
        hf.p[1].push_back(cvPoint(x + params[0], y + sz.height));
        hf.p[1].push_back(cvPoint(x + (params[0] << 1), y + params[0] + params[1]));
        hf.p[1].push_back(cvPoint(x + (params[0] << 1), y + params[0]));
        hf.p[1].push_back(cvPoint(x + params[0], y));
        hf.A[1] = hf.A[0];

        return hf;
    }


    EdgeType2::EdgeType2()
    {
        nparams = 2;
        can_swap = false;
        can_rotate90 = true;
        can_rotate180 = false;
    }


    // compute the size
    CvSize EdgeType2::size(const vector<int> & params)
    {
        CvSize sz;
        if(params[0] > 0 && params[1] >= 0)
        {
            sz.height = params[0] + params[1];
            sz.width = sz.height;
        }
        else
            sz.width = sz.height = -1;
        return sz;
    }


    // generate a PHaar feature with its top left at (x,y) and with its parameters given
    PHaarFeature EdgeType2::generate(int x, int y, const vector<int> & params)
    {
        PHaarFeature hf;
        int len = params[0] + params[1];

        hf.p[0].push_back(cvPoint(x + len, y));
        hf.p[0].push_back(cvPoint(x, y + len));
        hf.p[0].push_back(cvPoint(x, y + params[1]));
        hf.p[0].push_back(cvPoint(x + params[1], y));
        hf.p[0].push_back(cvPoint(x + len, y));
        hf.A[0] = 0.5 * params[0] * (params[0] + (params[1] << 1));

        hf.p[1].push_back(cvPoint(x + len, y));
        hf.p[1].push_back(cvPoint(x, y + len));
        hf.p[1].push_back(cvPoint(x + params[0], y + len));
        hf.p[1].push_back(cvPoint(x + len, y + params[0]));
        hf.p[1].push_back(cvPoint(x + len, y));
        hf.A[1] = hf.A[0];

        return hf;
    }


    Corner90Type1::Corner90Type1()
    {
        nparams = 2;
        can_swap = false;
        can_rotate90 = true;
        can_rotate180 = true;
    }


    // compute the size
    CvSize Corner90Type1::size(const vector<int> & params)
    {
        CvSize sz;
        if(params[0] > 0 && params[1] >= 0)
        {
            sz.height = (params[0] << 1) + params[1];
            sz.width = sz.height;
        }
        else
            sz.width = sz.height = -1;
        return sz;
    }


    // generate a PHaar feature with its top left at (x,y) and with its parameters given
    PHaarFeature Corner90Type1::generate(int x, int y, const vector<int> & params)
    {
        PHaarFeature hf;
        int l1 = params[0] + params[1];
        int len = params[0] + l1;

        hf.p[0].push_back(cvPoint(x + params[1], y));
        hf.p[0].push_back(cvPoint(x + params[1], y + params[1]));
        hf.p[0].push_back(cvPoint(x, y + params[1]));
        hf.p[0].push_back(cvPoint(x, y + l1));
        hf.p[0].push_back(cvPoint(x + l1, y + l1));
        hf.p[0].push_back(cvPoint(x + l1, y));
        hf.p[0].push_back(cvPoint(x + params[1], y));
        hf.A[0] = params[0] * (params[0] + (params[1] << 1));

        hf.p[1].push_back(cvPoint(x + l1, y));
        hf.p[1].push_back(cvPoint(x + len, y + params[0]));
        hf.p[1].push_back(cvPoint(x + len, y + len));
        hf.p[1].push_back(cvPoint(x + params[0], y + len));
        hf.p[1].push_back(cvPoint(x, y + l1));
        hf.p[1].push_back(cvPoint(x + l1, y + l1));
        hf.p[1].push_back(cvPoint(x + l1, y));
        hf.A[1] = params[0] * l1 * 2;

        return hf;
    }


    Corner90Type2::Corner90Type2()
    {
        nparams = 2;
        can_swap = false;
        can_rotate90 = true;
        can_rotate180 = true;
    }


    // compute the size
    CvSize Corner90Type2::size(const vector<int> & params)
    {
        CvSize sz;
        if(params[0] >= 0 && params[1] > 0)
        {
            sz.width = params[0] + (params[1] << 1);
            sz.height = (params[0] + params[1]) << 1;
        }
        else
            sz.width = sz.height = -1;
        return sz;
    }


    // generate a PHaar feature with its top left at (x,y) and with its parameters given
    PHaarFeature Corner90Type2::generate(int x, int y, const vector<int> & params)
    {
        PHaarFeature hf;
        int l1 = params[0] + params[1];

        hf.p[0].push_back(cvPoint(x, y));
        hf.p[0].push_back(cvPoint(x, y + params[1]));
        hf.p[0].push_back(cvPoint(x + params[0], y + l1));
        hf.p[0].push_back(cvPoint(x, y + l1 + params[0]));
        hf.p[0].push_back(cvPoint(x, y + (l1 << 1)));
        hf.p[0].push_back(cvPoint(x + l1, y + l1));
        hf.p[0].push_back(cvPoint(x, y));
        hf.A[0] = params[1] * (params[1] + (params[0] << 1));

        hf.p[1].push_back(cvPoint(x, y));
        hf.p[1].push_back(cvPoint(x + l1, y + l1));
        hf.p[1].push_back(cvPoint(x, y + (l1 << 1)));
        hf.p[1].push_back(cvPoint(x + params[1], y + (l1 << 1)));
        hf.p[1].push_back(cvPoint(x + l1 + params[1], y + l1));
        hf.p[1].push_back(cvPoint(x + params[1], y));
        hf.p[1].push_back(cvPoint(x, y));
        hf.A[1] = hf.A[0] + params[1] * params[1];

        return hf;
    }


    Corner45Type::Corner45Type()
    {
        nparams = 2;
        can_swap = true;
        can_rotate90 = true;
        can_rotate180 = true;
    }


    // compute the size
    CvSize Corner45Type::size(const vector<int> & params)
    {
        CvSize sz;
        if(params[0] > 0 && params[1] > 1)
        {
            int t3 = (int)round(params[1]*M_SQRT1_2);
            sz.height = params[0] + params[1] + t3;
            sz.width = sz.height + t3;
        }
        else
            sz.width = sz.height = -1;
        return sz;
    }


    // generate a PHaar feature with its top left at (x,y) and with its parameters given
    PHaarFeature Corner45Type::generate(int x, int y, const vector<int> & params)
    {
        PHaarFeature hf;
        CvSize sz = size(params);
        int t3 = sz.width - sz.height;

        hf.p[0].push_back(cvPoint(x, y + t3));
        hf.p[0].push_back(cvPoint(x, y + params[0] + t3));
        hf.p[0].push_back(cvPoint(x + params[0], y + params[0] + t3));
        hf.p[0].push_back(cvPoint(x, y + t3));
        hf.A[0] = 0.5 * params[0] * params[0];

        hf.p[1].push_back(cvPoint(x, y + t3));
        hf.p[1].push_back(cvPoint(x + t3, y));
        hf.p[1].push_back(cvPoint(x + sz.width, y + sz.height));
        hf.p[1].push_back(cvPoint(x, y + sz.height));
        hf.p[1].push_back(cvPoint(x, y + params[0] + t3));
        hf.p[1].push_back(cvPoint(x + params[0], y + params[0] + t3));
        hf.p[1].push_back(cvPoint(x, y + t3));
        hf.A[1] = 0.5 * sz.width * sz.width - hf.A[0] - t3 * t3;

        return hf;
    }


    Corner135Type::Corner135Type()
    {
        nparams = 2;
        can_swap = true;
        can_rotate90 = true;
        can_rotate180 = true;
    }


    // compute the size
    CvSize Corner135Type::size(const vector<int> & params)
    {
        CvSize sz;
        if(params[0] > 1 && params[1] > 1)
        {
            int t3 = (int)round(params[0]*M_SQRT1_2);
            int t4 = (int)round(params[1]*M_SQRT1_2);
            sz.width = params[0] + t3 + (t4 << 1);
            sz.height = (params[1] << 1) + t3;
        }
        else
            sz.width = sz.height = -1;
        return sz;
    }


    // generate a PHaar feature with its top left at (x,y) and with its parameters given
    PHaarFeature Corner135Type::generate(int x, int y, const vector<int> & params)
    {
        PHaarFeature hf;
        CvSize sz = size(params);
        int t3 = (int)round(params[0]*M_SQRT1_2);
        int t4 = (int)round(params[1]*M_SQRT1_2);
        int t5 = t3 + params[1] - t4;
        int t6 = sz.height - (t4 << 1);

        hf.p[0].push_back(cvPoint(x, y + params[1] + t3));
        hf.p[0].push_back(cvPoint(x + sz.width - t4 - t5, y + params[1] + t3));
        hf.p[0].push_back(cvPoint(x + sz.width - t4, y + t4));
        hf.p[0].push_back(cvPoint(x + params[0] + t3, y));
        hf.p[0].push_back(cvPoint(x + params[0], y + t3));
        hf.p[0].push_back(cvPoint(x, y + t3));
        hf.p[0].push_back(cvPoint(x, y + params[1] + t3));
        hf.A[0] = (params[0] + t3 + t4)*(params[1] + t3) - params[0]*t3 - 0.5*(t3*t3+t4*t4+t5*t5);

        hf.p[1].push_back(cvPoint(x, y + params[1] + t3));
        hf.p[1].push_back(cvPoint(x + sz.width - t4 - t5, y + params[1] + t3));
        hf.p[1].push_back(cvPoint(x + sz.width - t4, y + t4));
        hf.p[1].push_back(cvPoint(x + sz.width, y + (t4 << 1)));
        hf.p[1].push_back(cvPoint(x + sz.width - t6, y + sz.height));
        hf.p[1].push_back(cvPoint(x, y + sz.height));
        hf.p[1].push_back(cvPoint(x, y + params[1] + t3));
        hf.A[1] = sz.width*(sz.height - t4) - 0.5*(t4*t4+t5*t5+t6*t6) - (sz.width-t4-t5)*(sz.height-t4-params[1]);

        return hf;
    }


    CurveType1::CurveType1()
    {
        nparams = 3;
        can_swap = false;
        can_rotate90 = true;
        can_rotate180 = true;
    }


    // compute the size
    CvSize CurveType1::size(const vector<int> & params)
    {
        CvSize sz;
        if(params[0] > 0 && params[1] >= 0 && params[2] >= 0)
        {
            int t4 = (int)round(params[1]*M_SQRT1_2);
            sz.width = params[0] + (params[2] << 1) + (t4 << 2);
            sz.height = (params[1] << 1) + params[2];
        }
        else
            sz.width = sz.height = -1;
        return sz;
    }


    // generate a PHaar feature with its top left at (x,y) and with its parameters given
    PHaarFeature CurveType1::generate(int x, int y, const vector<int> & params)
    {
        PHaarFeature hf;
        CvSize sz = size(params);
        int t4 = (int)round(params[1]*M_SQRT1_2);
        int t5 = params[2] + params[1] - t4;
        int t6 = sz.height - (t4 << 1);

        hf.p[0].push_back(cvPoint(x + t4, y + t4));
        hf.p[0].push_back(cvPoint(x + t4 + t5, y + t4 + t5));
        hf.p[0].push_back(cvPoint(x + sz.width - t4 - t5, y + t4 + t5));
        hf.p[0].push_back(cvPoint(x + sz.width - t4, y + t4));
        hf.p[0].push_back(cvPoint(x + sz.width - (t4 << 1), y));
        hf.p[0].push_back(cvPoint(x + (t4 << 1) + params[0] + params[2], y + params[2]));
        hf.p[0].push_back(cvPoint(x + (t4 << 1) + params[2], y + params[2]));
        hf.p[0].push_back(cvPoint(x + (t4 << 1), y));
        hf.p[0].push_back(cvPoint(x + t4, y + t4));
        hf.A[0] = (params[0] + ((params[2] + t4) << 1))*(t4 + t5) - t5*t5 - t4*t4 - params[2]*params[2] - params[0]*params[2];

        hf.p[1].push_back(cvPoint(x + t4, y + t4));
        hf.p[1].push_back(cvPoint(x + t4 + t5, y + t4 + t5));
        hf.p[1].push_back(cvPoint(x + sz.width - t4 - t5, y + t4 + t5));
        hf.p[1].push_back(cvPoint(x + sz.width - t4, y + t4));
        hf.p[1].push_back(cvPoint(x + sz.width, y + (t4 << 1)));
        hf.p[1].push_back(cvPoint(x + sz.width - t6, y + sz.height));
        hf.p[1].push_back(cvPoint(x + t6, y + sz.height));
        hf.p[1].push_back(cvPoint(x, y + (t4 << 1)));
        hf.p[1].push_back(cvPoint(x + t4, y + t4));
        hf.A[1] = sz.width*(t4+t6) - t4*t4 - t5*t5 - t6*t6 - t5*(sz.width - ((t4+t5) << 1));

        return hf;
    }


    CurveType2::CurveType2()
    {
        nparams = 3;
        can_swap = false;
        can_rotate90 = true;
        can_rotate180 = true;
    }


    // compute the size
    CvSize CurveType2::size(const vector<int> & params)
    {
        CvSize sz;
        if(params[0] > 1 && params[1] > 1 && params[2] >= 0)
            sz.width = sz.height = params[0] + (params[1] << 1) + params[2];
        else
            sz.width = sz.height = -1;
        return sz;
    }


    // generate a PHaar feature with its top left at (x,y) and with its parameters given
    PHaarFeature CurveType2::generate(int x, int y, const vector<int> & params)
    {
        PHaarFeature hf;
        CvSize sz = size(params);
        int t4 = (int)round(params[1]*M_SQRT1_2);
        int d0 = params[0] + params[2];
        int d1 = params[0] + params[1] + params[2];
        int d2 = params[0] - params[1] + (t4 << 1);
        int d3 = params[0] - (params[1] << 1) + (t4 << 2);

        hf.p[0].push_back(cvPoint(x, y + d1));
        hf.p[0].push_back(cvPoint(x + d2, y + d1));
        hf.p[0].push_back(cvPoint(x + d1, y + d2));
        hf.p[0].push_back(cvPoint(x + d1, y));
        hf.p[0].push_back(cvPoint(x + d0, y));
        hf.p[0].push_back(cvPoint(x + d0, y + params[0]));
        hf.p[0].push_back(cvPoint(x + params[0], y + d0));
        hf.p[0].push_back(cvPoint(x, y + d0));
        hf.p[0].push_back(cvPoint(x, y + d1));
        hf.A[0] = d1*d1 - d0*d0 - 0.5*((d1-d2)*(d1-d2)-params[2]*params[2]);

        hf.p[1].push_back(cvPoint(x, y + d1));
        hf.p[1].push_back(cvPoint(x + d2, y + d1));
        hf.p[1].push_back(cvPoint(x + d1, y + d2));
        hf.p[1].push_back(cvPoint(x + d1, y));
        hf.p[1].push_back(cvPoint(x + sz.width, y));
        hf.p[1].push_back(cvPoint(x + sz.width, y + d3));
        hf.p[1].push_back(cvPoint(x + d3, y + sz.width));
        hf.p[1].push_back(cvPoint(x, y + sz.width));
        hf.p[1].push_back(cvPoint(x, y + d1));
        hf.A[1] = sz.width*sz.width - d1*d1 - 0.5*((sz.width-d3)*(sz.width-d3)-(d1-d2)*(d1-d2));

        return hf;
    }


    AbstractPHaarType *new_PHaarType(int type_index)
    {
        switch(type_index)
        {
            case 0: return new EdgeType1();
            case 1: return new EdgeType2();
            case 2: return new Corner90Type1();
            case 3: return new Corner90Type2();
            case 4: return new Corner45Type();
            case 5: return new Corner135Type();
            case 6: return new CurveType1();
            case 7: return new CurveType2();
        }

        return 0;
    }


    void test_type()
    {
        CurveType2 et1;

        IplImage *img = cvCreateImage(cvSize(100, 100), IPL_DEPTH_8U, 1 );
        cvSet(img, cvScalar(1));

        PolygonalIntegrator polyint(100, 100, 2);
        polyint.input_image(img);

        vector<int> params;
        params.resize(2, 0); // 2 elements

        int x, y, i, j;
        CvSize sz;
        PHaarFeature hf;
        double val;

        for(params[0] = 0; params[0] < 6; ++params[0]) for(params[1] = 0; params[1] < 6; ++params[1]) for(params[2] = 0; params[2] < 6; ++params[2])
        {
            sz = et1.size(params);
            if(sz.width < 0) continue;

            cout << "t1 = " << params[0] << ", t2 = " << params[1] << ", t3 = " << params[2] << ", sz=(" << sz.width << ", " << sz.height << ")" << endl;

            x = 10; y = 20;

            hf = et1.generate(x, y, params);


            for(i = 0; i < 2; ++i)
            {
                val = polyint.integrate(hf.p[i]);
                if(fabs(val) != hf.A[i])
                {
                    cout << "Found inconsistency:" << endl;
                    cout << "val=" << val << ", A[" << i << "]=" << hf.A[i] << endl;
                    for(j = 0; j < hf.p[0].size(); ++j)
                        cout << "P[" << i << "][" << j << "]=(" << hf.p[i][j].x << "," << hf.p[i][j].y << ")" << endl;
                }
            }
        }

        cvReleaseImage(&img);

        cvWaitKey(0);
    }


    void test_type2()
    {
        EdgeType2 et1;

        IplImage *img = cvCreateImage(cvSize(100, 100), IPL_DEPTH_8U, 1 );
        cvSet(img, cvScalar(1));

        PolygonalIntegrator polyint(100, 100, 2);
        polyint.input_image(img);

        vector<int> params;
        params.resize(2, 0); // 2 elements

        int x, y, i, j;
        CvSize sz;
        PHaarFeature hf;
        double val;

        for(params[0] = 0; params[0] < 6; ++params[0]) for(params[1] = 0; params[1] < 6; ++params[1])
        {
            sz = et1.size(params);
            if(sz.width < 0) continue;

            cout << "t1 = " << params[0] << ", t2 = " << params[1] << ", sz=(" << sz.width << ", " << sz.height << ")" << endl;

            x = 10; y = 20;

            hf = et1.generate(x, y, params);


            for(i = 0; i < 2; ++i)
            {
                val = polyint.integrate(hf.p[i]);
                if(fabs(val) != hf.A[i])
                {
                    cout << "Found inconsistency:" << endl;
                    cout << "val=" << val << ", A[" << i << "]=" << hf.A[i] << endl;
                    for(j = 0; j < hf.p[0].size(); ++j)
                        cout << "P[" << i << "][" << j << "]=(" << hf.p[i][j].x << "," << hf.p[i][j].y << ")" << endl;
                }
            }
        }

        cvReleaseImage(&img);

        cvWaitKey(0);
    }
}

